#include "RoadBuilder.h"
#include "refline/WorldRefLineBuilder.h"
#include "entities/WorldRoadObject.h"

rg::RoadBuilder::RoadBuilder(const FWorldEntityAxis& worldEntityAxis, const FWorldEntityManager& worldEntityMgr)
	: _axis(worldEntityAxis)
	, _em(worldEntityMgr)
	, _reflines(std::make_shared<FWorldRefLineGraph>())
{

}

void rg::RoadBuilder::BindOrCreate(const std::vector<FWorldRoad::Ptr>& roads, bool reverseAxisFlags[3])
{
	//default  is right handed
	bool leftHanded = false;
	leftHanded = reverseAxisFlags[0] ? !leftHanded : leftHanded;
	leftHanded = reverseAxisFlags[1] ? !leftHanded : leftHanded;
	leftHanded = reverseAxisFlags[2] ? !leftHanded : leftHanded;
	rg::Setting::leftHanded = leftHanded;
	// Build cubic hermite splines for reference lines.
	{
		double s[3];
		s[0] = reverseAxisFlags[0] ? -1.0 : 1.0;
		s[1] = reverseAxisFlags[1] ? -1.0 : 1.0;
		s[2] = reverseAxisFlags[2] ? -1.0 : 1.0;

		FWorldRefLineBuilder builder(_reflines, _axis,
			glm::dmat4x4(
				s[0], 0.0, 0.0, 0.0,
				0.0, s[1], 0.0, 0.0,
				0.0, 0.0, s[2], 0.0,
				0.0, 0.0, 0.0, 1.0
			));
		builder.Reset(roads);
		builder.Process();
	}

	for (const auto& r : roads)
	{
		BindOrCreate(r);
	}
	

	//not move the path in junction to the same height plane before stitch the roads
	std::unordered_map<int, std::vector<RoadData>> JunctionMap;
	for (auto road : _roadGeometries) {
		if (road.second.roadGeom->JunctionId == -1)
			continue;
		JunctionMap[road.second.roadGeom->JunctionId].push_back(road.second);
	}
	auto AreSame = [](double a, double b)->bool
	{
		return fabs(a - b) < 0.0001;
	};
	for (auto junctionRoads : JunctionMap)
	{
		//for every junction
		std::vector<glm::dvec3> refpointInJunction;
		double minx{ DBL_MAX }, maxx{ -DBL_MAX }, miny{ DBL_MAX }, maxy{ -DBL_MAX }, minz{ DBL_MAX }, maxz{ -DBL_MAX };
		
		for (auto roadData : junctionRoads.second)
		{
			RoadData* road = &roadData;
			road->roadGeom->GenerateLaneLines();
			for (auto& SectionLaneDivLines : road->roadGeom->mCachedLaneSectionDivLines)
			{
				for (auto& divline : SectionLaneDivLines)
				{			
					int size = refpointInJunction.size();
					refpointInJunction.resize(size + divline.second.samples.size());
					
					for(auto i=0; i< divline.second.samples.size(); i++)
					{
						auto& point = refpointInJunction[size + i];
						auto& p = divline.second.samples[i];
						point.x = p.P.x;
						point.y = p.P.y;
						point.z = p.E;

						point = point + road->roadGeom->mLocalOrigin;

						minx = minx > point.x ? point.x : minx;
						miny = miny > point.y ? point.y : miny;
						minz = minz > point.z ? point.z : minz;
						maxx = maxx < point.x ? point.x : maxx;
						maxy = maxy < point.y ? point.y : maxy;
						maxz = maxz < point.z ? point.z : maxz;
					}
				}
			}
		}

		double step = 0.1;
		int width = (maxx - minx) / step + 2;
		int height = (maxy - miny) / step + 2;
		
		double** heightMap;
		double** heightMapBlur;
		const double NaNHeight = -10000.0f;
		heightMap = new double *[height];
		heightMapBlur = new double *[height];
		for (int i = 0; i < height; i++)
		{
			heightMap[i] = new double[width];
			heightMapBlur[i] = new double[width];
			std::fill_n(heightMap[i], width, NaNHeight);
			std::fill_n(heightMapBlur[i], width, NaNHeight);
		}
		
		for (auto& p : refpointInJunction)
		{
			int i = (p.x - minx) / step + 1;
			int j = (p.y - miny) / step + 1;
			if (AreSame(heightMap[j][i], NaNHeight))
			{
				heightMap[j][i] = p.z;
			}
			else
			{
				heightMap[j][i] = (heightMap[j][i] + p.z)*0.5;
			}
		}
		int rangeSize = 20;
		for (int i = 0; i < height; i++) {
			for (int j = 0; j < width; j++) {
				if (AreSame(heightMap[i][j], NaNHeight)) //not usefull position
					continue;
				int startI = (i - rangeSize) > 0 ? i - rangeSize : 0;
				int endI = (i + rangeSize)<height? i + rangeSize:height-1;
				int startJ = (j - rangeSize)>0? j - rangeSize:0;
				int endJ = (j + rangeSize)<width ? j + rangeSize : width - 1;
				double total = 0;
				double totalweight = 0;
				for (int m = startI; m <= endI; m++)
				{
					for (int n = startJ; n <= endJ; n++)
					{
						double weight = abs(m - i) + abs(n - j) > rangeSize ? 0.5 : 1;
						if (!AreSame(heightMap[m][n], NaNHeight))
						{
							total += heightMap[m][n] * weight;
							totalweight += weight;
						}
					}
				}
				heightMapBlur[i][j] = total/totalweight;
			}
		}
		for (auto road : junctionRoads.second)
		{
			int sectionSize = road.roadGeom->mCachedLaneSectionDivLines.size();
			for (int i = 0; i< sectionSize; i++)
			{
				auto&SectionLaneDivLines = road.roadGeom->mCachedLaneSectionDivLines[i];
				for (auto& divline : SectionLaneDivLines)
				{
					rg::RefPoint start = divline.second.samples[0];
					rg::RefPoint end = divline.second.samples.back();
					glm::dvec3 p0 = glm::dvec3(start.P, start.E) + road.roadGeom->mLocalOrigin;
					glm::dvec3 p1 = glm::dvec3(end.P, end.E) + road.roadGeom->mLocalOrigin;
					
					for (auto j=0; j< divline.second.samples.size(); j++)
					{
						auto& sample = divline.second.samples[j];
						glm::dvec3 p = glm::dvec3(sample.P,sample.E) + road.roadGeom->mLocalOrigin;

						int x = (p.x - minx) / step + 1;
						int y = (p.y - miny) / step + 1;
						double weight1 = 1;
						if (i == 0)
						{
							//check distance to front
							double d = glm::distance(p, p0);
							if (d < 0.5)
								weight1 = 0;
							else
							{
								weight1 = (d - 0.5) / 2;
								weight1 = weight1 > 1 ? 1 : weight1;
							}
						}
						double weight2 = 1;
						if (i == sectionSize - 1)
						{
							//check distance to end
							double d = glm::distance(p, p1);
							if (d < 0.5)
								weight2 = 0;
							else
							{

								weight2 = (d - 0.5) / 2;
								weight2 = weight2 > 1 ? 1 : weight2;
							}

						}
						double weight = weight1 > weight2 ? weight2 : weight1;
						double bias = heightMapBlur[y][x] - sample.E;
						sample.E += bias * weight;
					}
				}
			}
		}
		
		for (int i = 0; i < height; i++)
		{
			delete [] heightMap[i];
			delete [] heightMapBlur[i];
		}
		delete[] heightMap;
		delete[] heightMapBlur;

	}

	//Now add the precessor&successor information in created ODRefLineComponent, which will be used to fill the precessor&successor 
	//information in ODRoadGeomActor later. Be noted we do not need the Junction Actor in UE right now, so the connection will only
	//be between roads(could be roads inside a junction)
	auto getRoadLaneLink = [](FWorldRoad* pRoad, bool Start, std::vector<std::pair<int, int>>& laneconnections)
	{
		auto sections = pRoad->Lanes->LaneSection;
		int size = (int)sections.size();
		if (size < 1)
			return;
		int index = Start ? 0 : size - 1;
		if (sections[index]->Center)
		{
			auto lanes = sections[index]->Center->Lane;
			for (auto lane : lanes)
			{
				if (lane->Link && lane->Link->Predecessor && lane->Link->Successor)
				{
					int laneId = lane->ID.Get();
					int linkLaneID = Start ? lane->Link->Predecessor->ID.Get() : lane->Link->Successor->ID.Get();
					laneconnections.push_back(std::make_pair(laneId, linkLaneID));
				}
			}
		}
		if (sections[index]->Left)
		{
			auto lanes = sections[index]->Left->Lane;
			for (auto lane : lanes)
			{
				if (lane->Link && lane->Link->Predecessor && lane->Link->Successor)
				{
					int laneId = lane->ID.Get();
					//NOTE: the land direction is different than road direction, so switch lane pre & succ, we only care about mesh pre&suc, not lane logic here.
					int linkLaneID = !Start ? lane->Link->Predecessor->ID.Get() : lane->Link->Successor->ID.Get();
					laneconnections.push_back(std::make_pair(laneId, linkLaneID));
				}
			}
		}
		if (sections[index]->Right)
		{
			auto lanes = sections[index]->Right->Lane;
			for (auto lane : lanes)
			{
				if (lane->Link && lane->Link->Predecessor && lane->Link->Successor)
				{
					int laneId = lane->ID.Get();
					int linkLaneID = Start ? lane->Link->Predecessor->ID.Get() : lane->Link->Successor->ID.Get();
					laneconnections.push_back(std::make_pair(laneId, linkLaneID));
				}
			}
		}
	};
	auto addRoadLink = [this](FWorldRoad* pRoad, FWorldRoad* pRoadLink, bool predecessor, const FWorldRoadConnectInfo& connectInfo)
	{
		//to add the road link for roadActor and store the laneLink in FWorldRoad

		auto roadData = _roadGeometries.find(pRoad);
		auto roadDataLink = _roadGeometries.find(pRoadLink);

		if (roadData != _roadGeometries.end() && roadDataLink != _roadGeometries.end())
		{
			if (predecessor)
			{
				roadData->second.predecessors.push_back(roadDataLink->second.roadGeom);
				pRoad->predecessors.push_back(std::make_pair(pRoadLink, connectInfo));
			}
			else
			{
				roadData->second.sucessors.push_back(roadDataLink->second.roadGeom);
				pRoad->sucessors.push_back(std::make_pair(pRoadLink, connectInfo));
			}
		}
	};
	auto addRoadLinkInsideJunction = [&getRoadLaneLink, &addRoadLink, this](FWorldRoad* pRoad, FWorldJunction* pJunction, bool precessor)
	{
		FWorldRoad* pRoadLink;
		//In our OD, Junction connection only stores the road link that goes to junction(not come from)
		//here precessor is junction, so we need to do extra search to find out the precessor path in side the junction
		for (auto& connection : pJunction->Connection)
		{
			FWorldRoad::Ptr incoming = *connection->IncomingRoad;
			FWorldRoad::Ptr connecting = *connection->ConnectingRoad;
			if (incoming->ID.Get() == pRoad->ID.Get())
			{
				//this road is incoming road, simply get the information
				pRoadLink = connecting.get();
				FWorldRoadConnectInfo connectInfo;
				connectInfo.contactpoint = connection->ContactPoint.Get();
				for (auto laneLink : connection->LaneLink)
				{
					connectInfo.laneconnections.push_back(std::make_pair(laneLink->From.Get(), laneLink->To.Get()));
				}
				addRoadLink(pRoad, pRoadLink, precessor, connectInfo);
			}
			else {
				//OD definition is not straight forward, even if we find the id in incoming, it still could be a connecting road too.
				auto successor = connecting->Link&&connecting->Link->Successor ? *connecting->Link->Successor->ElementId : nullptr;
				auto predecessor = connecting->Link&&connecting->Link->Predecessor ? *connecting->Link->Predecessor->ElementId : nullptr;
				if (successor&&successor->ID.Get() == pRoad->ID.Get())
				{
					pRoadLink = connecting.get();
					FWorldRoadConnectInfo connectInfo;
					connectInfo.contactpoint = FWorldContactPoint::End;
					//we can only get the lane link information from link path, and reverse it for us
					std::vector<std::pair<int, int>> laneconnections;
					getRoadLaneLink(pRoadLink, false, laneconnections);
					for (auto l : laneconnections)
					{
						connectInfo.laneconnections.push_back(std::make_pair(l.second, l.first));
					}
					addRoadLink(pRoad, pRoadLink, precessor, connectInfo);
				}
				if (predecessor&&predecessor->ID.Get() == pRoad->ID.Get())
				{
					pRoadLink = connecting.get();
					FWorldRoadConnectInfo connectInfo;
					connectInfo.contactpoint = FWorldContactPoint::Start;

					//we can only get the lane link information from link path, and reverse it for us
					std::vector<std::pair<int, int>> laneconnections;
					getRoadLaneLink(pRoadLink, true, laneconnections);
					for (auto l : laneconnections)
					{
						connectInfo.laneconnections.push_back(std::make_pair(l.second, l.first));
					}
					addRoadLink(pRoad, pRoadLink, precessor, connectInfo);
				}
			}
		}
	};

	//Get all the road&lane link information
	for (auto& roadRefLine : _roadGeometries)
	{
		FWorldRoad* pRoad = roadRefLine.first;
		if (pRoad&&pRoad->Link)
		{
			auto pConnPredecessor = pRoad->Link->Predecessor ? *pRoad->Link->Predecessor->ElementId : nullptr;
			if (pConnPredecessor && pConnPredecessor->IsA<FWorldRoad>())
			{
				FWorldRoadConnectInfo connectInfo;
				connectInfo.contactpoint = pRoad->Link->Predecessor->ContactPoint.Get();
				getRoadLaneLink(pRoad, true, connectInfo.laneconnections);

				addRoadLink(pRoad, pConnPredecessor->As<FWorldRoad>().get(), true, connectInfo);
			}
			if (pConnPredecessor && pConnPredecessor->IsA<FWorldJunction>())
			{
				auto pJunction = pConnPredecessor->As<FWorldJunction>().get();
				addRoadLinkInsideJunction(pRoad, pJunction, true);
			}

			auto pConnSuccessor = pRoad->Link->Successor ? *pRoad->Link->Successor->ElementId : nullptr;
			if (pConnSuccessor && pConnSuccessor->IsA<FWorldRoad>())
			{
				FWorldRoadConnectInfo connectInfo;
				connectInfo.contactpoint = pRoad->Link->Successor->ContactPoint.Get();
				getRoadLaneLink(pRoad, false, connectInfo.laneconnections);

				addRoadLink(pRoad, pConnSuccessor->As<FWorldRoad>().get(), false, connectInfo);
			}
			if (pConnSuccessor && pConnSuccessor->IsA<FWorldJunction>())
			{
				auto pJunction = pConnSuccessor->As<FWorldJunction>().get();
				addRoadLinkInsideJunction(pRoad, pJunction, false);
			}
		}
	}
	//TODO: add the missing lane link(it's optional for junction lane line, some OD might not add them)

	//Use the lane link information to stitch the road
	for (auto& roadRefLine : _roadGeometries)
	{
		FWorldRoad* pRoad = roadRefLine.first;
		auto roadGeometry = roadRefLine.second.roadGeom;
		roadGeometry->RoadId = (int)pRoad->ID.Get();
		for (auto predecessor : pRoad->predecessors)
		{
			FWorldRoad* linkRoad = predecessor.first;
			if (linkRoad->ID.Get() > pRoad->ID.Get())
			{
				//do not need to stitch road in both road, the one with bigger id should do the job
				continue;
			}
			FWorldRoadConnectInfo& linkInfo = predecessor.second;
			auto linkRoadLineComp = _roadGeometries.find(linkRoad);
			if (linkRoadLineComp == _roadGeometries.end())
				continue;

			auto linkRoadGeometry = linkRoadLineComp->second.roadGeom;
			linkRoadGeometry->GenerateLaneLines();
			glm::dvec3 offsetDiff = roadGeometry->mLocalOrigin - linkRoadGeometry->mLocalOrigin;
			auto lanelinks = linkInfo.laneconnections;

			double s = linkInfo.contactpoint == FWorldContactPoint::Start ? -DBL_MAX : DBL_MAX;
			for (auto laneLink : lanelinks)
			{
				rg::LaneCap laneCap;
				laneCap.laneIdx = laneLink.first;
				int laneConnectIndex = laneLink.second;
				laneCap.lanePoint = linkRoadGeometry->SampleLane(s, laneConnectIndex);
				laneCap.lanePoint2 = linkRoadGeometry->SampleLane(s, laneConnectIndex > 0 ? laneConnectIndex - 1 : laneConnectIndex + 1);

				laneCap.lanePoint.P -= glm::vec2(offsetDiff.x, offsetDiff.y);
				laneCap.lanePoint2.P -= glm::vec2(offsetDiff.x, offsetDiff.y);
				roadGeometry->mStartCaps.push_back(laneCap);
			}
		}
		for (auto sucessor : pRoad->sucessors)
		{
			FWorldRoad* linkRoad = sucessor.first;
			if (linkRoad->ID.Get() > pRoad->ID.Get())
			{
				continue;
			}
			FWorldRoadConnectInfo& linkInfo = sucessor.second;
			auto linkRoadLineComp = _roadGeometries.find(linkRoad);
			if (linkRoadLineComp == _roadGeometries.end())
				continue;

			auto& linkRoadGeometry = linkRoadLineComp->second.roadGeom;
			linkRoadGeometry->GenerateLaneLines();
			glm::dvec3 offsetDiff = roadGeometry->mLocalOrigin - linkRoadGeometry->mLocalOrigin;
			auto lanelinks = linkInfo.laneconnections;
			double s = linkInfo.contactpoint == FWorldContactPoint::Start ? -DBL_MAX : DBL_MAX;
			for (auto laneLink : lanelinks)
			{
				rg::LaneCap laneCap;
				laneCap.laneIdx = laneLink.first;
				int laneConnectIndex = laneLink.second;
				laneCap.lanePoint = linkRoadGeometry->SampleLane(s, laneConnectIndex);
				laneCap.lanePoint2 = linkRoadGeometry->SampleLane(s, laneConnectIndex > 0 ? laneConnectIndex - 1 : laneConnectIndex + 1);

				laneCap.lanePoint.P -= glm::vec2(offsetDiff.x, offsetDiff.y);
				laneCap.lanePoint2.P -= glm::vec2(offsetDiff.x, offsetDiff.y);
				roadGeometry->mEndCaps.push_back(laneCap);
			}
		}
	}
}

static rg::ReferenceLine SampleSpline(const FWorldRefLineSpline* spline, const glm::dvec3& offset, double threshold)
{
	rg::ReferenceLine ret;

	double splineLen = spline->Length();
	int sampleCnt = int(splineLen / threshold);
	if (sampleCnt == 0)
		sampleCnt = 1;

	for (int ci = 0; ci <= sampleCnt; ++ci) {

		double curDis = (double)ci / (double)sampleCnt * splineLen;
		glm::dvec3 p_interp = spline->EvalAtDistance(curDis) - offset;
		glm::dvec3 t_interp = spline->EvalTangentAtDistance(curDis);
		// This is to workaround a defect in UE spline evaluation, probably caused by having to spline points nearly at the same location.
		if (glm::length(t_interp) < (threshold * 0.0001f))
			continue;

		glm::vec2 roadRef_P(p_interp.x, p_interp.y);
		glm::vec2 roadRef_T(t_interp.x, t_interp.y);
		roadRef_T = glm::normalize(roadRef_T);
		// TODO: need to handle road elevation.
		ret.samples.push_back({ glm::vec3(p_interp), roadRef_T, curDis, 1.0f });
	}
	return ret;
}

void rg::RoadBuilder::BindOrCreate(const FWorldRoad::Ptr& road)
{
	FWorldRefLineSpline::Ptr spline = (*_reflines)[road];
	if (!spline) return;

	// Bind the reference line spline to UE component.

	// Offset the whole spline for more effective digits.
	glm::dbox3 bbox;
	glm::dvec3 curveCenter;

	for (const auto& span : spline->Get())
	{
		bbox |= span.P0;
		bbox |= span.P1;
	}
	curveCenter = bbox.center();
	auto& roadData = _roadGeometries[road.get()];
	if (!roadData.roadGeom) 
		roadData.roadGeom = std::make_shared<rg::RoadGeometry>();

	roadData.roadGeom->mLocalOrigin = curveCenter;

	if (road->ElevationProfile) {
		auto elevationProfile = road->ElevationProfile.Get();
		for (auto elevation : elevationProfile->Elevation) {
			rg::Elevation elev;
			elev.a = elevation->A;
			elev.b = elevation->B;
			elev.c = elevation->C;
			elev.d = elevation->D;
			elev.soffset = elevation->S;
			roadData.roadGeom->mElevationProfile.elevations.push_back(elev);
		}
	}

	if (road->Objects)
	{
		FWorldRoadObjects::Ptr roadObjects = road->Objects.Get();
		for (auto object : roadObjects->Object.Get())
		{
			auto roadObject = *(object->ID);
			if (roadObject->Type.Get() == "stop line")
			{
				//Confirmed with Qilin, the s of the stopline will be defined to the center of the line, so we don't need to consider different cases of finding the direction of the stop line
				//We will shift 1/2 width to get two lines to draw a mesh
				rg::StopLine stopline;
				//0.3 meter by default, confirmed with Qilin, would get from OD later
				stopline.width = 0.3f;
				stopline.s = (float)roadObject->S;
				stopline.t = (float)roadObject->T;
				stopline.pitch = (float)roadObject->Pitch;
				stopline.hdg = (float)roadObject->Hdg;
				stopline.roll = (float)roadObject->Roll;

				for (auto corner : roadObject->Outline->CornerLocal)
				{
					stopline.outline.push_back(glm::vec3(corner->V, corner->U, corner->Z));
				}
				roadData.roadGeom->mStopLines.push_back(stopline);
			}
			else if (roadObject->Type.Get() == "standard crosswalk")
			{
				rg::CrossWalk crossWalk;
				crossWalk.crossWalkWidth = 0.4f;//need to read from OD later, confirm with Qilin
				crossWalk.crossWalkGap = 0.2f;//need to read from OD later, confirm with Qilin
				crossWalk.s = (float)roadObject->S;
				crossWalk.t = (float)roadObject->T;
				crossWalk.pitch = (float)roadObject->Pitch;
				crossWalk.hdg = (float)roadObject->Hdg;
				crossWalk.roll = (float)roadObject->Roll;
				for (auto corner : roadObject->Outline->CornerLocal)
				{
					crossWalk.outline.push_back(glm::vec3(corner->V, corner->U, corner->Z));
				}
				roadData.roadGeom->mCrossWalks.push_back(crossWalk);
			}
		}
	}

	FWorldLanes::Ptr lanes = road->Lanes.Get();
	for (const auto& offset : lanes->LaneOffset)
		roadData.roadGeom->mLaneOffsets.push_back({ offset->S.Get(), offset->A.Get(), offset->B.Get(), offset->C.Get(), offset->D.Get() });

	auto toRGLane = [](const FWorldLane& worldLane) {
		rg::Lane ret;
		ret.laneID = worldLane.ID.Get();
		ret.is_driving = (worldLane.Type == FWorldLaneType::Driving);
		if (worldLane.Border.size() > 0) {
			// This lane width is specified by border property
			ret.using_border = true;
			for (const auto& b : worldLane.Border) {
				rg::LaneWidth curW = rg::LaneWidth(b->SOffset, b->A, b->B, b->C, b->D);
				ret.widths.push_back(curW);
			}
		}
		else {
			ret.using_border = false;
			for (const auto& w : worldLane.Width) {
				rg::LaneWidth curW = rg::LaneWidth(w->SOffset, w->A, w->B, w->C, w->D);
				ret.widths.push_back(curW);
			}
		}

		for (const auto& rm : worldLane.RoadMark) {
			rg::RoadMark curRM;
			curRM.width = (float)rm->Width.Get();
			curRM.soffset = (float)rm->SOffset.Get();

			curRM.type = rg::RoadMarkType::kSolid;
			switch (rm->Type.Get()) {
			case FWorldRoadMarkKind::Solid:
				curRM.type = rg::RoadMarkType::kSolid;
				break;
			case FWorldRoadMarkKind::Broken:
				curRM.type = rg::RoadMarkType::kBroken;
				break;
			case FWorldRoadMarkKind::SolidSolid:
				curRM.type = rg::RoadMarkType::kSolidSolid;
				break;
			case FWorldRoadMarkKind::BrokenSolid:
				curRM.type = rg::RoadMarkType::kBrokenSolid;
				break;
			case FWorldRoadMarkKind::BrokenBroken:
				curRM.type = rg::RoadMarkType::kBrokenBroken;
				break;
			case FWorldRoadMarkKind::SolidBroken:
				curRM.type = rg::RoadMarkType::kSolidBroken;
				break;
                        default :
                                break;
			}

			if (rm->Color == FWorldRoadMarkColor::Yellow)
				curRM.type |= rg::RoadMarkColor::kYellow;
			else
				curRM.type |= rg::RoadMarkColor::kWhite;

			if (curRM.width > 0)
				ret.roadMarks.push_back(curRM);
		}
		return ret;
	};

	for (const auto& laneSection : lanes->LaneSection) {
		rg::LaneSection rg_laneSection;
		rg_laneSection.s = laneSection->S;

		if (laneSection->Left.Get()) {
			for (const auto& leftLane : laneSection->Left->Lane) {
				rg_laneSection.leftLanes.push_back(toRGLane(*leftLane));
			}
		}

		if (laneSection->Right.Get()) {
			for (const auto& rightLane : laneSection->Right->Lane) {
				rg_laneSection.rightLanes.push_back(toRGLane(*rightLane));
			}
		}

		if (laneSection->Center.Get()) {
			for (const auto& centerLane : laneSection->Center->Lane) {
				rg_laneSection.centerLanes.push_back(toRGLane(*centerLane));
			}
		}
		roadData.roadGeom->mLaneSections.push_back(rg_laneSection);
	}

	roadData.roadGeom->mCheckRewind = false;
	roadData.roadGeom->mOptimizeGoemetry = true;
	roadData.roadGeom->RoadId = road->ID.Get();
	roadData.roadGeom->JunctionId = road->Junction.Get();
	// Need to take the component scaling into account.
	rg::ReferenceLine roadRefLine = SampleSpline(spline.get(), curveCenter, 0.1f);
	roadData.roadGeom->Initialize(roadRefLine, 0.1f);
}
